function [specVelInc, angleInc] = corimumeas(mode, specVelInc, angleInc, angleInc_1, IMUPar, dpFull, mask, DTGgSensSymmetry, Dt) %#codegen
%CORIMUMEAS 利用器件参数误差修正比速度增量和角度增量
%
% Input Arguments:
% mode: 修正算法模式，int8(1)表示1阶算法计算修正量，int8(2)表示偏微分算法计算修正量
% specVelInc: m*3矩阵，m为采样数，修正前的比速度增量，单位为m/s
% angleInc: m*3矩阵，修正前的角度增量，单位为rad
% angleInc_1: m*3矩阵，前一周期修正后的角增量，单位为rad
% IMUPar: 惯组误差参数结构体
% dpFull: m*36矩阵，器件参数误差
% mask: 1*36的逻辑向量，器件参数屏蔽向量，true表示包含该器件参数，依次为加速度计的3个零偏误差、3个标度因数误差、
% 6个失准角误差、3个二次项系数误差和陀螺的3个零偏误差、3个标度因数误差、6个失准角误差、9个g敏感项误差，
% 单位分别为m/s^2、ppm、rad、1/(m/s^2)、rad/s、ppm、rad、(rad/s)/(m/s^2)
% DTGgSensSymmetry: 1*9的int8类型向量，用于指示挠性惯组g敏感项误差中的对称性，在mask指示包含的g敏感项误差中，
% DTGgSensSymmetry元素与包含参数相同的为该包含参数的正对称参数，相反的为该包含参数的负对称参数，
% 0表示不存在对称参数，仅考虑第一个对称参数
% Dt: 标量，采样周期，单位s
%
% Output Arguments:
% specVelInc: m*3矩阵，修正后的比速度增量，单位为m/s
% angleInc: m*3矩阵，修正后的角度增量，单位为rad
%
% References:
% [1] 理论文档 版本号1.0 章节号9.5“导航参数的修正”

m = size(dpFull, 1);
% 挠性惯组对称性处理
for i=1:9
    if mask(1, 27+i) && (DTGgSensSymmetry(1, i)~=0)
        for j=1:9
            if j ~= i
                if DTGgSensSymmetry(1, j) == DTGgSensSymmetry(1, i)
                    dpFull(:, 27+j) = dpFull(:, 27+i);
                    break;
                elseif DTGgSensSymmetry(1, j) == -DTGgSensSymmetry(1, i)
                    dpFull(:, 27+j) = -dpFull(:, 27+i);
                    break;
                end
            end
        end
    end
end
% REF1式9.168
%陀螺
IMUParDiff.gyro.KScal0 = zeros(3,1);
IMUParDiff.gyro.PS0B = eye(3);
IMUParDiff.gyro.domegaIBBiasS = dpFull(:, 16:18)';
IMUParDiff.gyro.dKScalP = dpFull(:, 19:21)';
IMUParDiff.gyro.dKScalN = zeros(3,1);
IMUParDiff.gyro.dPSS0 = vec2offdiag(dpFull(:, 22:27))';
IMUParDiff.gyro.domegaIBQuantS = zeros(3,1);
IMUParDiff.gyro.DGSens = permute(reshape(dpFull(:, 28:36)', 3, 3, m), [2 1 3]);
%加速度计
IMUParDiff.acc.KScal0 = zeros(3,1);
IMUParDiff.acc.PS0B = eye(3);
IMUParDiff.acc.dfBiasS = dpFull(:, 1:3)';
IMUParDiff.acc.dKScalP = [dpFull(:, 4:6)' dpFull(:, 13:15)'];
IMUParDiff.acc.dKScalN = zeros(3,2);
IMUParDiff.acc.dPSS0 = vec2offdiag(dpFull(:, 7:12))';
IMUParDiff.acc.lB = zeros(3,3);
IMUParDiff.acc.dfQuantS = zeros(3,1);
IMUParDiff.acc.KAniso = zeros(3,1);
IMUParDiff.acc.CBA = zeros(3,3,3);
%误差修正
if mode == int8(1)
    [ dDalphaBl,  ~ ] = gyroerror_inc( angleInc', IMUParDiff.gyro, Dt, specVelInc');
    [ dDupsilonBl, ~ ] = accerror_inc( specVelInc', IMUParDiff.acc, Dt, angleInc', zeros(3,1));
    angleInc = angleInc - dDalphaBl';
    specVelInc = specVelInc - dDupsilonBl';
else
    [tmp_specVelInc, tmp_angleInc] = imumeascorr_pdiff_inc(IMUParDiff, IMUPar, specVelInc', angleInc', Dt, angleInc_1');% TODO\LD:未测试，处理实际数据时溢出
    specVelInc = tmp_specVelInc';
    angleInc = tmp_angleInc';
end
end